/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "movefreeinstructionlogic.h"

#include "programset.h"
#include "mccontrollerthread.h"
#include "programengine.h"
#include "GeneralDefine.h"
#include "messagelog.h"
#include "fleetservice.h"

extern FleetService *fleetService;

MoveFreeInstructionLogic::MoveFreeInstructionLogic(int robotIdIn, ProgramSet *programSetIn, ProgramEngine *programEngineIn,
                               McControllerThread *programMcControllerIn, QMap<int, DPoint> *vehicleDPointIn)
{
    robotId=robotIdIn;
    programSet=programSetIn;
    programEngine=programEngineIn;
    programMcController=programMcControllerIn;
    vehicleDPoint=vehicleDPointIn;
    lastMotionInstructionLine=-1;
//    waitFlag=false;
    cu1PositionAllowAngleAccuracy=0.1;

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString xmlfile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )
                                                    +"/config/mcControllerInterpreter.xml";
    int tmpReturn=readConfigFile(xmlfile);
    if(1!=tmpReturn)
    {
        addMsg( ENUM_MSG_ERROR, "MoveFreeInstructionLogic", "MoveFreeInstructionLogic", 3905, robotId);
    }

//    initialTestPoint();//just for test
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::startMoveFree(int programNumIn,
                                                              const MoveFreeInstruction &instructionIn,
                                                              int lineIndex)
{

    E_PROGRAM_LOOP_RESULT tmpResult;
    DPoint tmpPot;
    switch(instructionIn.pointArryType)
    {
    case E_MOVE_FREE_POINTS_ARRY_DP:
        tmpResult=moveToPointByPathStyle_blockWait(instructionIn,lineIndex,false,tmpPot,programNumIn);
        break;
    case E_MOVE_FREE_POINTS_ARRY_DPR:
        tmpResult=moveToPointByAutoBarrier_blockWait(instructionIn,lineIndex,false,tmpPot,programNumIn);
        break;
    case E_MOVE_FREE_POINTS_ARRY_DPL:
        tmpResult=moveToPointByBestPathMap_blockWait(instructionIn,lineIndex,programNumIn);
        break;
    default:
        qDebug()<<"error,,,,instructionIn.pointArryType"<<instructionIn.pointArryType;
        return E_PROGRAM_LOOP_RESULT_ERROR;

    }

    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
    {
        return tmpResult;
    }
    programEngine->calculateNextLineToRun(programNumIn);
    return programEngine->isTheLastInstruction(programNumIn);
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::moveToPointByPathStyle_blockWait(const MoveFreeInstruction &instructionIn,
                                        int lineIndex,bool isDirectPoint,DPoint targetPoint,int programNumIn)
{
    E_PROGRAM_LOOP_RESULT resultKey=moveSetting(instructionIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    resultKey=addProgramMoveCommand(instructionIn,lineIndex,isDirectPoint,targetPoint,programNumIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    resultKey=waitMotionAccomplish(instructionIn,lineIndex,isDirectPoint,targetPoint,programNumIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    quitClear();
    return E_PROGRAM_LOOP_RESULT_NORMAL;

}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::moveToPointByAutoBarrier_blockWait(const MoveFreeInstruction &instructionIn,
                                        int lineIndex, bool isDirectPoint, DPoint targetPoint, int programNumIn)
{
    qDebug()<<"MoveFreeInstructionLogic::moveToPointByAutoBarrier_blockWait";
    E_PROGRAM_LOOP_RESULT resultKey;

    programMcController->mcControllerInterpreter->setMotionServerModel
                                         (ENUM_MOTION_MODEL_VEHICLE_ROS_CONTROL);

    //加点
    VehiclePosition position;
    if(isDirectPoint)
    {
        position.x=targetPoint.posX;
        position.y=targetPoint.posY;
        position.rotate=targetPoint.posR;
    }
    else
    {
        if(!vehicleDPoint->contains(instructionIn.registerPointsValue))
        {
            quitClear();
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3906, robotId
                    , instructionIn.registerPointsValue);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        string mapNameReturn;
        programMcController->mcControllerInterpreter->getCurrentMapName(mapNameReturn);
    //            qDebug()<<"mapNameReturn"<<mapNameReturn<<"recordMap"<<vehicleDPoint->value(instructionIn.pointsValue).mapName;
        string recordMap=vehicleDPoint->value(instructionIn.registerPointsValue).mapName;

        if(0!=QString::fromStdString(mapNameReturn).compare(QString::fromStdString(recordMap)))
        {
            quitClear();
            qDebug()<<"8888888888888888"<<QString::fromStdString(mapNameReturn)<<"[]"<<QString::fromStdString(recordMap);
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3911, robotId
                    , programEngine->getRunningLineFloat(),0,0,0,QString::fromStdString(
                        recordMap) );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        position.x=vehicleDPoint->value(instructionIn.registerPointsValue).posX;
        position.y=vehicleDPoint->value(instructionIn.registerPointsValue).posY;
        position.rotate=vehicleDPoint->value(instructionIn.registerPointsValue).posR;

    }
    int returnFlag;
    programMcController->mcControllerInterpreter->moveToPosition(position,1,returnFlag);
    if(1!=returnFlag)
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    resultKey=waitMovebaseTargetAccomplish(position,instructionIn.allowAccuracy);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    quitClear();
    return E_PROGRAM_LOOP_RESULT_NORMAL;

}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::moveToPointByBestPathMap_blockWait(const MoveFreeInstruction &instructionIn,
                                                                                   int lineIndex, int programNumIn)
{
    qDebug()<<"MoveFreeInstructionLogic::moveToPointByBestPathMap_blockWait";
    E_PROGRAM_LOOP_RESULT resultKey=moveSetting_pathRoute(instructionIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }
    //获取最优路径
    fleetService->clearFleetCommandQueue();
    UserInfo userInfo;
    AutoDriveParameter paraIn;
    paraIn.speedRatio=instructionIn.CNT_Value/100.0;
    paraIn.targetStationId=instructionIn.trackMoveFileName.toInt();
    if(1!=fleetService->addAutoDrive(userInfo,paraIn))
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    QQueue<FleetCommand> fleetCommandQueueOut;
    fleetService->getFleetCommandQueue(fleetCommandQueueOut);

    E_PROGRAM_LOOP_RESULT tmpResult;
    while(1)
    {
        usleep(5000);
        int tmpKey=processAutoDrivePathQueue(fleetCommandQueueOut);
        if(0==tmpKey)
        {
            programMcController->mcControllerInterpreter->setRunningNumber_virtualLineControl(lineIndex+0.5);
            tmpResult=E_PROGRAM_LOOP_RESULT_NORMAL;
            break;
        }
        else   if(0>tmpKey)
        {
            programMcController->mcControllerInterpreter->setRunningNumber_virtualLineControl(lineIndex+0.5);
            tmpResult=E_PROGRAM_LOOP_RESULT_ERROR;
            break;
        }
        else   if(1==tmpKey)
        {

        }
        else
        {
            programMcController->mcControllerInterpreter->setRunningNumber_virtualLineControl(lineIndex+0.5);
            tmpResult=E_PROGRAM_LOOP_RESULT_CEASE_STOP;
            break;
        }
    }

    //复位行号
    programMcController->mcControllerInterpreter->setRunningNumber_virtualLineControl(lineIndex+0.9);
    return tmpResult;


}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::moveToPointByPathStyle_noWait(const MoveFreeInstruction &instructionIn, int lineIndex,
                                                                   bool isDirectPoint, DPoint targetPoint, int programNumIn)
{
    E_PROGRAM_LOOP_RESULT resultKey=moveSetting(instructionIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    resultKey=addProgramMoveCommand(instructionIn,lineIndex,isDirectPoint,targetPoint,programNumIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::moveToMultiPointByPathStyle_run(const QVector<int> &lineList,
                           const QVector<LaserPathParameter> &laserPathListParaIn, int isLastPointMove)
{
    if(0>=laserPathListParaIn.size())
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    MoveFreeInstruction instruction;
    instruction.instructionType=MOVE_FREE;
    instruction.moveFreeMode=laserPathListParaIn[0].moveFreeMode;
    instruction.pointArryType=laserPathListParaIn[0].pointArryType;
    instruction.pointSensorType=laserPathListParaIn[0].pointSensorType;
    instruction.trackMoveFileName=laserPathListParaIn[0].trackMoveFileName;
    instruction.velocityRatio=laserPathListParaIn[0].velocityRatio;
    instruction.pointsValue=laserPathListParaIn[0].pointsValue;
    instruction.pointSensorType=laserPathListParaIn[0].pointSensorType;
    instruction.registerPointsValue=laserPathListParaIn[0].registerPointsValue;
    instruction.CNT_Value=laserPathListParaIn[0].CNT_Value;
    instruction.moveFreeMoveTrajectoryType=laserPathListParaIn[0].moveFreeMoveTrajectoryType;
    instruction.isPositiveDirection=laserPathListParaIn[0].isPositiveDirection;
    instruction.allowAccuracy=laserPathListParaIn[0].allowAccuracy;
    instruction.pointIndex=laserPathListParaIn[0].pointIndex;
    instruction.laserTypeNum=laserPathListParaIn[0].laserTypeNum;
    instruction.isDetectBarrier=laserPathListParaIn[0].isDetectBarrier;
    instruction.barrierRunMode=laserPathListParaIn[0].barrierRunMode;
    instruction.pathControlStrategy=laserPathListParaIn[0].pathControlStrategy;


    E_PROGRAM_LOOP_RESULT resultKey=moveSetting(instruction);//waitMotionAccomplish_fleet会设置每一段的壁障策略。
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    resultKey=addProgramMoveCommand_fleet(lineList,laserPathListParaIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    resultKey=waitMotionAccomplish_fleet(lineList);//如果出错会跳出去在这里。这时候需要停止或清除旧轨迹，才可以退出。
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }

    quitClear();
    return E_PROGRAM_LOOP_RESULT_NORMAL;

}

int MoveFreeInstructionLogic::moveToNavgationPoint(MovingToNavagationParameter parameterIn, int &returnFlag)
{
    qDebug()<<"MoveFreeInstructionLogic::moveToNavgationPoint"<<" x"<<parameterIn.x<<" y"<<parameterIn.y
              <<" r"<<parameterIn.rotate<<" path"<<parameterIn.moveFreeMoveTrajectoryType
                <<" vel"<<parameterIn.speedRatio;
    //先复位算法，再启动算法。
    programMcController->mcControllerInterpreter->resetAll();
    programMcController->mcControllerInterpreter->allStartRun();

    MoveFreeInstruction instructionIn;
    static int lineIndex=0;
    lineIndex++;
    bool isDirectPoint=true;
    DPoint targetPoint;
    targetPoint.posX=parameterIn.x;
    targetPoint.posY=parameterIn.y;
    targetPoint.posR=parameterIn.rotate;

    instructionIn.instructionType=MOVE_FREE;              // 指令类型

    instructionIn.moveFreeMode=(E_MOVE_FREE_MODE )parameterIn.moveFreeMode;                 // 自由导航模式

    instructionIn.pointArryType=E_MOVE_FREE_POINTS_ARRY_DP;    // 自由导航点位类型

    instructionIn.pointSensorType=(MOVE_FREE_POINT_TYPE )parameterIn.pointSensorType;      // 自由导航指令点位模式

    instructionIn.trackMoveFileName="";                      // 轨道运行文件路径

    instructionIn.velocityRatio=100*parameterIn.speedRatio;                          // 运行速度比

    instructionIn.pointsValue=0;                            // DP单点数值

    instructionIn.registerPointsValue=0;                    // DPR寄存器点数值

    instructionIn.noteStr="";                                // 注释

    instructionIn.CNT_Value=parameterIn.CNT_Value;                                  // 平滑数值  当CNT_Value=0:FINE

    instructionIn.moveFreeMoveTrajectoryType=(ENUM_MOVE_FREE_MOVE_TRAJECTORY_TYPE )parameterIn.moveFreeMoveTrajectoryType;   // 自由导航指令运行轨迹类型；  AUTO:自由轨迹  LINE:直线轨迹  SPLINE:样条轨迹

    instructionIn.isPositiveDirection=parameterIn.isPositiveDirection; //正向、反向运行。华磊
    instructionIn.allowAccuracy=parameterIn.allowAccuracy;//停车精度，单位米

    instructionIn.pointIndex=parameterIn.pointSensorType;

    instructionIn.laserTypeNum=parameterIn.laserTypeNum;

    instructionIn.jmpLabel="";

    instructionIn.isDetectBarrier=parameterIn.isDetectBarrier; //是否探测障碍物
    instructionIn.barrierRunMode=parameterIn.barrierRunMode;//探测方法

    instructionIn.pathControlStrategy=(E_PATH_CONTROL_STRATEGY)parameterIn.pathControlStrategy;

    if(E_PROGRAM_LOOP_RESULT_NORMAL!=moveToPointByPathStyle_noWait(instructionIn,lineIndex,isDirectPoint,targetPoint))
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3914, robotId);
        returnFlag=-1;
        return 0;
    }
    returnFlag=1;
    return 1;
}

int MoveFreeInstructionLogic::isMotionInstructionFinished( int waitLine)
{
    float lineStatus;
    float floatLineDifference;
    lineStatus=programMcController->mcControllerInterpreter->getRunningNumber_virtualLineControl();
    floatLineDifference=lineStatus-waitLine;
//    qDebug()<<"lineStatus="<<lineStatus<<",waitLine="<<waitLine;
    //cnt????
//    if(1==programMcController->mcControllerInterpreter->isVirtualLineTrajectoryEmpty())
//    {
        //限制小于０．９９是因为回到第一行后，要保证行号逻辑正确．
//        if(floatLineDifference > 0.8 && floatLineDifference < 0.99)//?????fine类型指令，肯定是运行完一条才会加下一行。
        if(floatLineDifference > 0.8 )
        {
            return 1;
        }
//    }

    return -1;
}

int MoveFreeInstructionLogic::isTargetPointArrived(VehiclePosition targetPoint,double allowBias)
{
    //判断距离
    E_GOAL_STATUS tmpstatus;
    programMcController->mcControllerInterpreter->getMovingStatus(tmpstatus);
    switch(tmpstatus)
    {
    case E_GOAL_STATUS_SUCCEEDED:
        return 1;
        break;
    case E_GOAL_STATUS_LOST:
        return -1;
        break;
    }
    return 0;

}

int MoveFreeInstructionLogic::processMotionInstruction_oneMove(const MoveFreeInstruction &instructionIn, int startLine,
                                                       bool isDirectPoint,DPoint targetPoint)
{
    qDebug()<<"MoveFreeInstructionLogic::processMotionInstruction_oneMove";

    if(1!=programMcController->mcControllerInterpreter->isVirtualLineTrajectoryEmpty())
    {
        qDebug()<<"error, 1!=isVirtualLineTrajectoryEmpty 轨迹不为空";
        return -1;
    }
    else
    {
        qDebug()<<"1==isVirtualLineTrajectoryEmpty 轨迹为空";
    }

    QVector<NavParameter> tmpCommandVector;
    NavParameter tmpParameter;
    tmpParameter.moveType=(E_NAV_COMMAND)instructionIn.moveFreeMoveTrajectoryType;
    tmpParameter.velocityUnit=ENUM_RATIO;
//    tmpParameter.controlPointRatio1=D_SPLINE_CONTROL_RATIO;
//    tmpParameter.controlPointRatio2=D_SPLINE_CONTROL_RATIO;
//    tmpParameter.isRotatePrepare=false;

    tmpParameter.velocity=instructionIn.velocityRatio/100.0;
    if(false==isDirectPoint)
    {
        if(!vehicleDPoint->contains(instructionIn.pointsValue))
        {
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3900, robotId
                    , programEngine->getRunningLineFloat() );
            return -1;
        }
        tmpParameter.targetPoint.posX=vehicleDPoint->value(instructionIn.pointsValue).posX;
        tmpParameter.targetPoint.posY=vehicleDPoint->value(instructionIn.pointsValue).posY;
        tmpParameter.targetPoint.posR=vehicleDPoint->value(instructionIn.pointsValue).posR;
        tmpParameter.targetPoint.dpIndex=instructionIn.pointsValue;

        string mapNameReturn;
        programMcController->mcControllerInterpreter->getCurrentMapName(mapNameReturn);
//        qDebug()<<"mapNameReturn"<<mapNameReturn<<"recordMap"<<vehicleDPoint->value(instructionIn.pointsValue).mapName;
         string recordMap=vehicleDPoint->value(instructionIn.pointsValue).mapName;
        if(0!=QString::fromStdString(mapNameReturn).compare(QString::fromStdString(recordMap)))
        {
            qDebug()<<"98888888888888888"<<QString::fromStdString(mapNameReturn)<<"[]"<<QString::fromStdString(recordMap);

            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3911, robotId
                    , programEngine->getRunningLineFloat(),0,0,0,QString::fromStdString(
                        recordMap) );
            return -1;
        }
    }
    else
    {
      tmpParameter.targetPoint= targetPoint;
    }

    tmpParameter.cntRatio=0;
    tmpParameter.lineNumber=startLine;
    tmpParameter.incFlag=0;
    tmpParameter.pathFlag=0;
    tmpParameter.stationId=-1;
    tmpParameter.isPositiveDirection=instructionIn.isPositiveDirection;

    tmpCommandVector.append(tmpParameter);

    programMcController->mcControllerInterpreter->addVirtualLineTrajectoryCommand(tmpCommandVector);
    return 1;
}

int MoveFreeInstructionLogic::processMotionInstruction_multiMove(int programNum, int startLine)
{
//    qDebug()<<"MoveFreeInstructionLogic::processMotionInstruction_multiMove";
    programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_BEGIN);
    //判断如果队列为空，则一定要开始加入。如果不为空，则说明是暂停状态后恢复，不加。isVirtualLineTrajectoryEmpty的判断还是不太准确。
    //在线段末尾极限，暂停后，再次运行，判断为不为空。


        if(isNeedAddTraj(programNum,startLine))
        {
            //情况1：本行加轨迹
            if(1!=programMcController->mcControllerInterpreter->isVirtualLineTrajectoryEmpty())
            {//宏友出现运行状态一直停，就是这里的逻辑有问题。
                qDebug()<<"error, 1!=isVirtualLineTrajectoryEmpty 轨迹不为空";
                return -1;
            }
            else
            {
//                qDebug()<<"1==isVirtualLineTrajectoryEmpty 轨迹为空";
            }
        }
        else if(1==programMcController->mcControllerInterpreter->isVirtualLineTrajectoryEmpty())
        {
            //轨迹为空也必须加

        }
        else
        {
            qDebug()<<"本行不用加轨迹";
            return 1;
        }



    //平滑运动指令段时，首条加入队列，其余条根据上下文判断是否已经加入过。
    //单条运动指令时，只加入一条进队列，不做预处理

    //判断以前是否加入过该条指令,考虑到出错后轨迹被清空，所以要重新加入。
//    for(int i = startLine; i >0;i--)
//    {
//        //cnt平滑判断
//        if(MOVE_FREE == (*programSet)[programNum][i-1].type && 0!=(*programSet)[programNum][i-1].moveFreeInstruction.CNT_Value)
//        {
//            programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_CNT_POINT_BEFORE);

//            return 1;
//        }
//        else if(MOVE_FREE == (*programSet)[programNum][i-1].type && 0==(*programSet)[programNum][i-1].moveFreeInstruction.CNT_Value)
//        {
//            programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_NEW_POINT_BEGIN);

//            break;
//        }

//    }

    //只加１条或一段平滑
    QVector<NavParameter> tmpCommandVector;
    NavParameter tmpParameter;
    tmpParameter.velocityUnit=ENUM_RATIO;
//    tmpParameter.controlPointRatio1=D_SPLINE_CONTROL_RATIO;
//    tmpParameter.controlPointRatio2=D_SPLINE_CONTROL_RATIO;
//    tmpParameter.isRotatePrepare=false;

    for(int j = startLine; j < (*programSet)[programNum].count();j++)
    {
        if(MOVE_FREE == (*programSet)[programNum][j].type)
        {
            tmpParameter.moveType=(E_NAV_COMMAND)(*programSet)[programNum][j].moveFreeInstruction.moveFreeMoveTrajectoryType;
            tmpParameter.velocityUnit=ENUM_RATIO;
            tmpParameter.velocity=(*programSet)[programNum][j].moveFreeInstruction.velocityRatio/100.0;
            if(!vehicleDPoint->contains((*programSet)[programNum][j].moveFreeInstruction.pointsValue))
            {
                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3906, robotId
                        , (*programSet)[programNum][j].moveFreeInstruction.pointsValue);
                return -1;
            }
            string mapNameReturn;
            programMcController->mcControllerInterpreter->getCurrentMapName(mapNameReturn);
//            qDebug()<<"mapNameReturn"<<mapNameReturn<<"recordMap"<<vehicleDPoint->value(instructionIn.pointsValue).mapName;
            string recordMap=vehicleDPoint->value((*programSet)[programNum][j].moveFreeInstruction.pointsValue).mapName;

            if(0!=QString::fromStdString(mapNameReturn).compare(QString::fromStdString(recordMap)))
            {
                qDebug()<<"8888888888888888"<<QString::fromStdString(mapNameReturn)<<"[]"<<QString::fromStdString(recordMap);
                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3911, robotId
                        , programEngine->getRunningLineFloat(),0,0,0,QString::fromStdString(
                            recordMap) );
                return -1;
            }
            tmpParameter.targetPoint.posX=vehicleDPoint->value((*programSet)[programNum][j].moveFreeInstruction.pointsValue).posX;
            tmpParameter.targetPoint.posY=vehicleDPoint->value((*programSet)[programNum][j].moveFreeInstruction.pointsValue).posY;
            tmpParameter.targetPoint.posR=vehicleDPoint->value((*programSet)[programNum][j].moveFreeInstruction.pointsValue).posR;
            tmpParameter.targetPoint.dpIndex=(*programSet)[programNum][j].moveFreeInstruction.pointsValue;
            tmpParameter.cntRatio=(*programSet)[programNum][j].moveFreeInstruction.CNT_Value;
            tmpParameter.lineNumber=j;
            tmpParameter.incFlag=0;
            tmpParameter.pathFlag=0;
            tmpParameter.stationId=-1;
            tmpParameter.isPositiveDirection=(*programSet)[programNum][j].moveFreeInstruction.isPositiveDirection;

            if(E_NAV_COMMAND_SPLINE!=tmpParameter.moveType && 0!=tmpParameter.cntRatio)
            {
                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3913, robotId,j);
                return -1;
            }
            tmpCommandVector.append(tmpParameter);
            if(0==tmpParameter.cntRatio )
            {
                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }
        }
    }


    if( tmpCommandVector.size() )
    {
        programEngine->setMotionInstructionAdded(tmpCommandVector.size());
        programMcController->mcControllerInterpreter->addVirtualLineTrajectoryCommand(tmpCommandVector);
    }
    else
    {
        programEngine->setMotionInstructionAdded(0);
    }

    return 1;
}


int MoveFreeInstructionLogic::reset()
{
    lastMotionInstructionLine=-1;
//    waitFlag=false;
    pathSettingInfo.clear();
    return 1;
}


int MoveFreeInstructionLogic::getSetMotionBlock(int startLineNum)
{

}

int MoveFreeInstructionLogic::pretreatMotion()
{

}

bool MoveFreeInstructionLogic::isNeedAddTraj(int programNum, int startLine)
{

    for(int j = startLine-1; j >=0;j--)
    {
        if(MOVE_FREE == (*programSet)[programNum][j].type)
        {

            if(0==(*programSet)[programNum][j].moveFreeInstruction.CNT_Value)
            {
                return true;
            }

            if(0!=(*programSet)[programNum][j].moveFreeInstruction.CNT_Value)
            {
                return false;
            }
        }
    }
    return true;

}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::moveSetting(const MoveFreeInstruction &instructionIn)
{
    qDebug()<<"MoveFreeInstructionLogic::moveSetting";
    //设置停车精度；距离0.02时，角度3度
    programMcController->mcControllerInterpreter->setAgvTargetAllowOffset(
                instructionIn.allowAccuracy,2.6*instructionIn.allowAccuracy);
    //设置轨迹策略
    programMcController->mcControllerInterpreter->setAgvPathFollowStrategy(instructionIn.pathControlStrategy);
    programMcController->mcControllerInterpreter->setOriginalLaserFilterType(E_LASER_FILTER_TYPE_NONE);

    //设置是否检测障碍物
    if(false==instructionIn.isDetectBarrier)
    {
//        programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(false);
//        programMcController->mcControllerInterpreter->setPointCloudCheckEnable(false);
        programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_NONE);
    }
    else
    {
//        programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(true);
//        programMcController->mcControllerInterpreter->setPointCloudCheckEnable(true);
        programMcController->mcControllerInterpreter->setBarrierRunMode((E_BARRIER_RUN_MODE)instructionIn.barrierRunMode);
    }

    programMcController->mcControllerInterpreter->setMotionServerModel
                                         (ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);
    //odom set
    int tmpReturn;
    if(0==instructionIn.moveFreeMode)
    {
        programMcController->mcControllerInterpreter->setOdomSolver(E_ODOM_SOLVER_ENCODER,tmpReturn);
        programMcController->mcControllerInterpreter->setAmclIsCalculateLaser(false);
    }
    else
    {
        //programMcController->mcControllerInterpreter->setAmclIsCalculateLaser(true);
    }
//    else if(2==instructionIn.moveFreeMode)
//    {
//        programMcController->mcControllerInterpreter->setOdomSolver(E_ODOM_SOLVER_LASER,tmpReturn);
//    }


//    programMcController->mcControllerInterpreter->startOneModel(ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);



    // 普通点
    if(EM_MOVE_FREE_DP==instructionIn.pointSensorType)
    {

        if(1!=programMcController->mcControllerInterpreter->changeToPositionType(E_VEHICLE_POSITION_TYPE_ROS))
        {
            quitClear();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3904, robotId
                    , programEngine->getRunningLineFloat() );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        programMcController->mcControllerInterpreter->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS,true);
//        programMcController->mcControllerInterpreter->setCheckVehiclePosition_vmark(false);
//        programMcController->mcControllerInterpreter->setCheckVehiclePosition_amcl(true);
        programMcController->mcControllerInterpreter->setVehicleAcceleration(agv_lineMaxAcceleration);
        programMcController->mcControllerInterpreter->setVehicleAcceleration_rotate(agv_rotateMaxAcceleration);
        programMcController->mcControllerInterpreter->setVirtualLineEnableKpChange(true);
//        programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_COMMON);


    }
    // VMark点
    else if(EM_MOVE_FREE_VMARK == instructionIn.pointSensorType)
    {
        //激光扫描范围.切换可能有问题，ｖmark没有及时识别到。默认使用０号，为什么切换后等待５０ms都没有用。
        if(1!=programMcController->mcControllerInterpreter->setLaserIntensityFilterByType_visionMatch(instructionIn.laserTypeNum))
        {
            quitClear();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3902, robotId
                    , programEngine->getRunningLineFloat() );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }

        programMcController->mcControllerInterpreter->setCurrentVMarkType(instructionIn.pointIndex);
        if(1!=programMcController->mcControllerInterpreter->changeToPositionType(E_VEHICLE_POSITION_TYPE_VMARK))
        {
            quitClear();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3903, robotId
                    , programEngine->getRunningLineFloat() );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        programMcController->mcControllerInterpreter->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_VMARK,true);
//        programMcController->mcControllerInterpreter->setCheckVehiclePosition_vmark(true);
//        programMcController->mcControllerInterpreter->setCheckVehiclePosition_amcl(false);
        programMcController->mcControllerInterpreter->setVehicleAcceleration(vmark_lineAcceleration);
        programMcController->mcControllerInterpreter->setVehicleAcceleration_rotate(vmark_rotateAcceleration);
        programMcController->mcControllerInterpreter->setVirtualLineEnableKpChange(false);
//        programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_VMARK);

    }
    // 二维码点
    else if(EM_MOVE_FREE_QR==instructionIn.pointSensorType)
    {
        if(1!=programMcController->mcControllerInterpreter->changeToPositionType(E_VEHICLE_POSITION_TYPE_QRCODE))
        {
            quitClear();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3909, robotId
                    , programEngine->getRunningLineFloat() );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        programMcController->mcControllerInterpreter->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_QRCODE,true);
        programMcController->mcControllerInterpreter->setVehicleAcceleration(vmark_lineAcceleration);
        programMcController->mcControllerInterpreter->setVehicleAcceleration_rotate(vmark_rotateAcceleration);
        programMcController->mcControllerInterpreter->setVirtualLineEnableKpChange(false);
//        programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_VMARK);
    }
    // CU1点
    else if(EM_MOVE_FREE_CU1==instructionIn.pointSensorType)
    {
        //多次尝试.不能连续２次
//        int tmpTryCount=2;
        int isSucess;
//        do
//        {
            isSucess=programMcController->mcControllerInterpreter->changeToPositionType(E_VEHICLE_POSITION_TYPE_CU1);
//            QCoreApplication::processEvents();
//            QThread::sleep(1);
//            tmpTryCount--;
//            qDebug()<<"change to EM_MOVE_FREE_CU1 try "<<tmpTryCount;
//        }
//        while(1!=isSucess && tmpTryCount>0);

        if(1!=isSucess)
        {
            quitClear();
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3910, robotId
                    , programEngine->getRunningLineFloat() );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        programMcController->mcControllerInterpreter->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_CU1,true);
        programMcController->mcControllerInterpreter->setVehicleAcceleration(reflect_lineAcceleration);
        programMcController->mcControllerInterpreter->setVehicleAcceleration_rotate(reflect_rotateAcceleration);
        programMcController->mcControllerInterpreter->setVirtualLineEnableKpChange(false);
        //设置停车精度；
        programMcController->mcControllerInterpreter->setAgvTargetAllowOffset(
                    instructionIn.allowAccuracy,cu1PositionAllowAngleAccuracy*M_PI/180.0);
//        programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_VMARK);
    }
    else
    {
        quitClear();
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3908, robotId
                , programEngine->getRunningLineFloat() );
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::moveSetting_pathRoute(const MoveFreeInstruction &instructionIn)
{
    qDebug()<<"MoveFreeInstructionLogic::moveSetting";
    //设置停车精度；距离0.02时，角度3度
    programMcController->mcControllerInterpreter->setAgvTargetAllowOffset(
                instructionIn.allowAccuracy,2.6*instructionIn.allowAccuracy);
    //设置轨迹策略
    programMcController->mcControllerInterpreter->setAgvPathFollowStrategy(instructionIn.pathControlStrategy);
    programMcController->mcControllerInterpreter->setOriginalLaserFilterType(E_LASER_FILTER_TYPE_NONE);

    //设置是否检测障碍物
    if(false==instructionIn.isDetectBarrier)
    {
//        programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(false);
//        programMcController->mcControllerInterpreter->setPointCloudCheckEnable(false);
        programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_NONE);
    }
    else
    {
//        programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(true);
//        programMcController->mcControllerInterpreter->setPointCloudCheckEnable(true);
        programMcController->mcControllerInterpreter->setBarrierRunMode((E_BARRIER_RUN_MODE)instructionIn.barrierRunMode);
    }

    programMcController->mcControllerInterpreter->setMotionServerModel
                                         (ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);
    //odom set
    int tmpReturn;
    if(0==instructionIn.moveFreeMode)
    {
        programMcController->mcControllerInterpreter->setOdomSolver(E_ODOM_SOLVER_ENCODER,tmpReturn);
        programMcController->mcControllerInterpreter->setAmclIsCalculateLaser(false);
    }
    else
    {
        //programMcController->mcControllerInterpreter->setAmclIsCalculateLaser(true);
    }

    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::addProgramMoveCommand_fleet(const QVector<int> &lineList,
                                                         const QVector<LaserPathParameter> &laserPathListParaIn)
{
    if(0==laserPathListParaIn.size())
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }
//    qDebug()<<"888addProgramMoveCommand_fleet"<<lineList;
//    for(int i=0;i<lineList.size();i++)
//    {
//        qDebug()<<"lineList:"<<lineList[i];
//    }


    //调度系统平滑的每一个小段共用相同的参数。脚本程序是每个小段有不同的控制参数，比如壁障模式。
    MoveFreeInstruction tmpInstruction;
    tmpInstruction.allowAccuracy=laserPathListParaIn.front().allowAccuracy;
    tmpInstruction.barrierRunMode=laserPathListParaIn.front().barrierRunMode;
    tmpInstruction.isDetectBarrier=laserPathListParaIn.front().isDetectBarrier;
    tmpInstruction.laserTypeNum=laserPathListParaIn.front().laserTypeNum;
    tmpInstruction.pointIndex=laserPathListParaIn.front().pointIndex;
    tmpInstruction.pathControlStrategy=laserPathListParaIn.front().pathControlStrategy;
    tmpInstruction.moveFreeMode=E_MOVE_FREE_IMU;
    tmpInstruction.pointSensorType=laserPathListParaIn.front().pointSensorType;
    E_PROGRAM_LOOP_RESULT resultKey=moveSetting(tmpInstruction);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
    {
        return resultKey;
    }
    //(加入指令，非平滑加一条，平滑指令加一完整段。)
    QVector<NavParameter> tmpCommandVector;
    NavParameter tmpParameter;
    tmpParameter.velocityUnit=ENUM_RATIO;

    for(int j = 0; j < laserPathListParaIn.size();j++)
    {

            tmpParameter.moveType=(E_NAV_COMMAND)laserPathListParaIn[j].moveFreeMoveTrajectoryType;
            tmpParameter.velocityUnit=ENUM_RATIO;
            tmpParameter.velocity=laserPathListParaIn[j].velocityRatio/100.0;
            tmpParameter.targetPoint.posX=laserPathListParaIn[j].targetPosition.x*1000.0;//单位ｍｍ
            tmpParameter.targetPoint.posY=laserPathListParaIn[j].targetPosition.y*1000.0;;//单位ｍｍ
            tmpParameter.targetPoint.posR=laserPathListParaIn[j].targetPosition.rotate*180.0/M_PI;//单位degree
            qDebug()<<"targetPoint x="<<tmpParameter.targetPoint.posX<<"mm y="<<tmpParameter.targetPoint.posY
                                 <<"mm r="<<tmpParameter.targetPoint.posR<<"degree";
            tmpParameter.cntRatio=laserPathListParaIn[j].CNT_Value;
            tmpParameter.lineNumber=lineList[j];
            tmpParameter.incFlag=0;
            tmpParameter.pathFlag=0;
            tmpParameter.isPositiveDirection=laserPathListParaIn[j].isPositiveDirection;
            tmpParameter.stationId=laserPathListParaIn[j].stationId;
            tmpParameter.pathId=laserPathListParaIn[j].pathId;
//            tmpParameter.controlPointRatio1=laserPathListParaIn[j].controlPointRatio1;
//            tmpParameter.controlPointRatio2=laserPathListParaIn[j].controlPointRatio2;
//            tmpParameter.isRotatePrepare=laserPathListParaIn[j].isRotatePrepare;
//            tmpParameter.prepareAngle=laserPathListParaIn[j].prepareAngle;

            tmpParameter.p1x=laserPathListParaIn[j].p1x;
            tmpParameter.p1y=laserPathListParaIn[j].p1y;
            tmpParameter.p2x=laserPathListParaIn[j].p2x;
            tmpParameter.p2y=laserPathListParaIn[j].p2y;

            if(D_MIN_NAV_CNT>tmpParameter.cntRatio && 0!=tmpParameter.cntRatio)
            {
//                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3907, robotId
                        , D_MIN_NAV_CNT);
                qDebug()<<"D_MIN_NAV_CNT>tmpParameter.cntRatio && 0!=tmpParameter.cntRatio, cntRatio "
                       <<tmpParameter.cntRatio;
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            tmpCommandVector.append(tmpParameter);

            if(0==tmpParameter.cntRatio && j!=laserPathListParaIn.size()-1)
            {
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3907, robotId
                        , D_MIN_NAV_CNT);
                qDebug()<<"0==tmpParameter.cntRatio && j!=laserPathListParaIn.size()-1,j "<<j;
                return E_PROGRAM_LOOP_RESULT_ERROR;
//                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }

            if(false==pathSettingInfo.contains(laserPathListParaIn[j].pathId))
            {
                pathSettingInfo.insert(laserPathListParaIn[j].pathId,laserPathListParaIn[j]);
            }

    }


    if( tmpCommandVector.size() )
    {
//        programEngine->setMotionInstructionAdded(tmpCommandVector.size());
        programMcController->mcControllerInterpreter->addVirtualLineTrajectoryCommand(tmpCommandVector);
    }
    else
    {
//        programEngine->setMotionInstructionAdded(0);
    }

    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::addProgramMoveCommand(const MoveFreeInstruction &instructionIn,
                                                                      int lineIndex,bool isDirectPoint,
                                                                      DPoint targetPoint,int programNumIn)
{
    if(E_MOVE_FREE_MOVE_TRAJECTORY_SPLINE==instructionIn.moveFreeMoveTrajectoryType)
    {
        quitClear();
        qDebug()<<"E_MOVE_FREE_MOVE_TRAJECTORY_SPLINE==instructionIn.moveFreeMoveTrajectoryType";
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3915, robotId
                , programEngine->getRunningLineFloat() );
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    //添加轨迹指令
    int intMotionLine= programMcController->mcControllerInterpreter->getRunningNumber_virtualLineControl();
    if(intMotionLine!=lineIndex && true==isDirectPoint)
    {
        //充电用,发直接点DPoint. hmi调试用.
        if(1!=processMotionInstruction_oneMove(instructionIn,lineIndex,isDirectPoint,targetPoint))
        {
            quitClear();
            qDebug()<<"MoveFreeInstructionLogic1 startMotion error,,,processMotionInstruction_oneMove";
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3912, robotId
                    , programEngine->getRunningLineFloat() );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
    }
    else
    {
        //用户程序解释用
        if(1!=processMotionInstruction_multiMove(programNumIn,lineIndex))
        {
            quitClear();
            qDebug()<<"MoveFreeInstructionLogic2 startMotion error,,,processMotionInstruction_multiMove";
            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3912, robotId
                    , programEngine->getRunningLineFloat() );
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
    }
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::waitMotionAccomplish_fleet(const QVector<int> &lineList)
{
    for(int i=0;i<lineList.size();i++)
    {
        E_PROGRAM_LOOP_RESULT resultKey=waitMotionAccomplish_fleet(lineList[i]);

        if(E_PROGRAM_LOOP_RESULT_NORMAL!=resultKey)
        {
            return resultKey;
        }
    }
    return E_PROGRAM_LOOP_RESULT_NORMAL;
}


E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::waitMotionAccomplish_fleet(int lineIndex)
{
    //完成判断，没有则阻塞等待
    int isFinished=-1;

    while(-1==isFinished)
    {
        //判断当前的站点和路径，修改避障策略等参数。
        PathMovingInfo pathMoveingInfo;
        programMcController->mcControllerInterpreter->getPathMovingInfo_virtualLineControl(pathMoveingInfo);
        if(true==pathSettingInfo.contains(pathMoveingInfo.currentPathId))
        {
            if(false==pathSettingInfo.value(pathMoveingInfo.currentPathId).isDetectBarrier)
            {
                programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_NONE);
                pathSettingInfo.remove(pathMoveingInfo.currentPathId);
            }
            else
            {
                programMcController->mcControllerInterpreter->setBarrierRunMode(
                            (E_BARRIER_RUN_MODE)pathSettingInfo.value(pathMoveingInfo.currentPathId).barrierRunMode);
                pathSettingInfo.remove(pathMoveingInfo.currentPathId);
            }

        }
        else
        {
//            qDebug()<<"error,no found barrier info"<<pathMoveingInfo.currentPathId;

        }

//        waitFlag=true;
        if(1==isMotionInstructionFinished(lineIndex))
        {
            isFinished=1;
        }

        if(-1==isFinished)
        {
            usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
        }

        if(ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {
            quitClear();
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }

        if(programEngine->getQuickStopFlag())
        {
            quitClear();
            qDebug()<<"getQuickStopFlag";

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }

    }

    return  E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::waitMotionAccomplish(const MoveFreeInstruction &instructionIn,
                                                    int lineIndex,bool isDirectPoint,DPoint targetPoint,int programNumIn)
{
    //完成判断，没有则阻塞等待
    int isFinished=-1;

    while(-1==isFinished)
    {
//        waitFlag=true;
        if(1==isMotionInstructionFinished(lineIndex))
        {
            isFinished=1;
        }

        if(-1==isFinished)
        {
            usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
        }
        //修改为暂停不要退出。
        if(ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {
            quitClear();
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(ENUM_TRAJECTORY_STOPPED==programMcController->mcControllerInterpreter->getStatus())
        {
//            if(false==isInFleeting)
//            {
                if(ENUM_COMMAND_RESET != programEngine->getEngineStatue())
                {
                    quitClear();
                    qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                    addMsg( ENUM_MSG_REMIND, "ProgramEngine", "ProgramEngine",  3901, robotId
                            , programEngine->getRunningLineFloat() );
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
                programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(true);
                programMcController->mcControllerInterpreter->setPointCloudCheckEnable(true);
                return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
//            }

        }
        else if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue())//报错后，轨迹被清空，需要重新添加.
        {
            if(programEngine->getQuickStopFlag())
            {
                if(1==programMcController->mcControllerInterpreter->isVirtualLineTrajectoryEmpty())
                {
                    if(1!=isMotionInstructionFinished(lineIndex))//一定要加行有没有完成判断
                    {
                        //virtualLineControl在时间轴为０的时候，会不更新updateTrajectory,导致调试信息显示不正确。
                        addProgramMoveCommand(instructionIn,lineIndex,isDirectPoint,targetPoint,programNumIn);
                    }

                }
            }
        }
    }
//    waitFlag=false;

    return  E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT MoveFreeInstructionLogic::waitMovebaseTargetAccomplish(VehiclePosition targetPoint,double allowBias)
{
    //完成判断，没有则阻塞等待
    int isFinished=-1;

    while(-1==isFinished)
    {
//        waitFlag=true;
        int tmpTargetResult=isTargetPointArrived(targetPoint,allowBias);
        switch(tmpTargetResult)
        {
        case 1:
            isFinished=1;
            break;
        case 0:
            break;
        case -1:
            quitClear();
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_ERROR;
        default:
        {

        }
        }

        if(-1==isFinished)
        {
            usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
        }
        //修改为暂停不要退出。
        if(ENUM_COMMAND_RESET == programEngine->getEngineStatue())
        {
            quitClear();
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(ENUM_COMMAND_CEASE == programEngine->getEngineStatue())//报错后，轨迹被清空，需要重新添加.
        {
            quitClear();
            qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();

           return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
    }
//    waitFlag=false;

    return  E_PROGRAM_LOOP_RESULT_NORMAL;
}

int MoveFreeInstructionLogic::readConfigFile(QString fileName)
{
    qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 初始化配置文件：" << fileName;

    DomParser domparser;
    bool ok = domparser.openXml(fileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 配置文件打开失败：" << fileName;
        }
        return -1;
    }

    QDomNode domNode_f = domparser.findSubNode(domparser.getRootDomElement(), "robotSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取robotSetting结点失败：" << fileName;
        }
            return -2;
    }


    QDomNode domNode = domparser.findSubNode(domNode_f, "agv_lineMaxAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取agv_lineMaxAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, agv_lineMaxAcceleration);

    domNode = domparser.findSubNode(domNode_f, "agv_rotateMaxAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取agv_rotateMaxAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, agv_rotateMaxAcceleration);

    domNode_f = domparser.findSubNode(domparser.getRootDomElement(), "InstructionSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取InstructionSetting结点失败：" << fileName;
        }
            return -2;
    }


    domNode = domparser.findSubNode(domNode_f, "vmark_lineAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取vmark_lineAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, vmark_lineAcceleration);

    domNode = domparser.findSubNode(domNode_f, "vmark_rotateAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取vmark_rotateAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, vmark_rotateAcceleration);


    domNode = domparser.findSubNode(domNode_f, "reflect_lineAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取reflect_lineAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, reflect_lineAcceleration);

    domNode = domparser.findSubNode(domNode_f, "reflect_rotateAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取reflect_rotateAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, reflect_rotateAcceleration);

    domNode = domparser.findSubNode(domNode_f, "cu1PositionAllowAngleAccuracy", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, cu1PositionAllowAngleAccuracy);


    domparser.closeXml();
    return 1;

}


int MoveFreeInstructionLogic::initialTestPoint()
{
    vehicleDPoint=new QMap <int, DPoint>;
    vehicleDPoint->clear();
    DPoint point1;
    point1.dpIndex=1;
    point1.posX=600;
    point1.posY=1130;
    point1.posR=0;
    vehicleDPoint->insert(point1.dpIndex,point1);

    DPoint point2;
    point2.dpIndex=2;
    point2.posX=2600;
    point2.posY=360;
    point2.posR=0;
    vehicleDPoint->insert(point2.dpIndex,point2);

    DPoint point3;
    point3.dpIndex=3;
    point3.posX=2600;
    point3.posY=360;
    point3.posR=180;
    vehicleDPoint->insert(point3.dpIndex,point3);

    DPoint point4;
    point4.dpIndex=4;
    point4.posX=1000;
    point4.posY=1000;
    point4.posR=90;
    vehicleDPoint->insert(point4.dpIndex,point4);

    return 1;
}

int MoveFreeInstructionLogic::quitClear()
{
//    programMcController->mcControllerInterpreter->setUltrasonicSensorCheckEnable(true);
//    programMcController->mcControllerInterpreter->setPointCloudCheckEnable(true);
    programMcController->mcControllerInterpreter->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS,true);
    //调度模式会有问题。
//    programMcController->mcControllerInterpreter->setBarrierRunMode(E_BARRIER_RUN_MODE_COMMON);

    programMcController->mcControllerInterpreter->setAmclIsCalculateLaser(true);
    pathSettingInfo.clear();
    return 1;
}

int MoveFreeInstructionLogic::getCommandFromQueue(QQueue<FleetCommand> &fleetCommandQueue,FleetCommand &moveParameter)
{

    if(fleetCommandQueue.isEmpty())
    {
        return 0;
    }
    moveParameter = fleetCommandQueue.dequeue();

    return 1;
}

int MoveFreeInstructionLogic::processAutoDrivePathQueue(QQueue<FleetCommand> &fleetCommandQueue)
{

    FleetCommand tmp_fleet_command;
    if(0==getCommandFromQueue(fleetCommandQueue,tmp_fleet_command))
    {
        return 0;
    }

    int processResult=-1;

    QVector<LaserPathParameter> moveParameterArry;
    QVector<int> lineArry;

    switch(tmp_fleet_command.commandType)
    {

    case E_FLEET_COMMAND_LASER_PATH:
    {
        if(0==tmp_fleet_command.laserPathPara.CNT_Value)
        {//单条
            moveParameterArry.append(tmp_fleet_command.laserPathPara);
            lineArry.append(tmp_fleet_command.commandIndex);
//                    processResult=processNavLineContinusInstruction(lineArry,moveParameterArry,0);
            return moveToMultiPointByPathStyle_run(lineArry,moveParameterArry,0);
            break;
        }
        else
        {//多条平滑
            moveParameterArry.append(tmp_fleet_command.laserPathPara);
            lineArry.append(tmp_fleet_command.commandIndex);
            do
            {
                int tmpKey=getCommandFromQueue(fleetCommandQueue,tmp_fleet_command);
                if(0!=tmpKey)
                {
                   if(E_FLEET_COMMAND_LASER_PATH!=tmp_fleet_command.commandType)
                   {
                       addMsg(ENUM_MSG_ERROR,"FleetService","FleetService",12205,robotId);
                       return -1;
                   }
                   lineArry.append(tmp_fleet_command.commandIndex);
                   moveParameterArry.append(tmp_fleet_command.laserPathPara);
                   if(0==tmp_fleet_command.laserPathPara.CNT_Value)
                   {
//                              return processNavLineContinusInstruction(lineArry,moveParameterArry,0);
                      return moveToMultiPointByPathStyle_run(lineArry,moveParameterArry,0);
                   }

                }
                usleep(5000);

            }while(1);


             break;
        }
    }


//            case E_FLEET_COMMAND_MAGANETIC_PATH:
//            {
//                processResult=processMaganeticInstruction(tmp_fleet_command.maganeticPathPara);
//                break;

//            }


    default:
    {
        qDebug()<<"ignor autodrive commandType:"<<tmp_fleet_command.commandType;
        return 1;
    }

    }


    return 1;
}





void MoveFreeInstructionLogic::addMsg(int messageLevel, string componentName, string messageType, int messageCode,
                                      int robotId, int parameter1, int parameter2, int parameter3, int parameter4,
                                      QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;

    QString infomationStr;

    switch(messageCode)
    {
    case 3915:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("，不支持样条曲线样式！");

        break;
    }
    case 3914:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("，运动到导航点失败！");

        break;
    }
    case 3913:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，非样条曲线轨迹，不支持平滑cnt参数！");

        break;
    }
    case 3912:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，轨迹不为空，无法加入新的轨迹！");

        break;
    }
    case 3911:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，程序点的地图名称(%1)和当前地图名称不一致）！").arg(message);

        break;
    }
    case 3910:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，切换到反光定位模式失败）！")+message;

        break;
    }
    case 3909:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，切换到二维码定位模式失败）！")+message;

        break;
    }
    case 3908:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，没有指明运动点的类型）！")+message;

        break;
    }
    case 3907:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　请将导航指令的平滑系数修改为大于%1ｍｍ）！").arg(parameter1)+message;

        break;
    }
    case 3906:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第%1号点数据为空）！").arg(parameter1)+message;

        break;
    }
    case 3905:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　运动配置文件读取失败(逻辑指令部分））！")+message;

        break;
    }
    case 3904:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，切换到普通定位模式失败，普通定位正在初始化中，请稍等）！")+message;

        break;
    }
    case 3903:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，切换到Ｖ槽模式失败）！")+message;

        break;
    }
    case 3902:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行，激光扫描类型设置错误）！")+message;

        break;
    }
    case 3901:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("算法模块异常进入停止状态）！")+message;

        break;
    }
    case 3900:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr(",　第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（DP点数据不存在）！")+message;

        break;
    }


    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);

}
